import time

import machine
from machine import Pin, PWM, ADC, I2C

from rocker import ROCKER
from ssd1306 import SSD1306_I2C
# 超频到240Mhz
machine.freq(240000000)

servo_pwm = [PWM(Pin(21)), PWM(Pin(22)), PWM(Pin(23)), PWM(Pin(24))]  # 舵机pwm控制
rocker = ROCKER()  # 初始化摇杆
key_bus = ADC(28)  # 按键总线
scr = SSD1306_I2C(128, 64, I2C(0, sda=Pin(4), scl=Pin(5)))
scr.fill(0)
scr.show()

for i in range(0, 1000):
    scr.fill(0)
    # scr.text("X : " + rocker.getX() + " %", 0, 12)
    # scr.text("Y : " + rocker.getY() + " %", 0, 24)
    # scr.text("Btn : " + rocker.getBtn(), 0, 36)
    scr.text("KB : " + str(key_bus.read_u16()) + " %", 0, 12)
    scr.show()
    time.sleep_ms(200)

